    #include <iostream>
#include "ndarray.h"
#include <vector>
#include <list>
#include <deque>
#include <queue>
#include <map>
#include <boost/unordered_set.hpp>
#include <set>
#include <limits>  
#include <functional>
#include "boost/shared_ptr.hpp"
#include "boost/bind.hpp"
#include <boost/foreach.hpp>
#include <math.h>
#include "utils.h"


extern "C" {
  
#define POTTS(X, Y)  ((X) == (Y) ? (0) : (1))
#define BETA 3
#define ALPHA 1 

// this offset is used in case energie is negative, in fact we cannot garantee that the unary energie is always positive which mean the pdf could exceed 1!!!!      
#define offset 10     
//#define DEGUB    

    
  const int POSITIVE_INFINITY = std::numeric_limits<int>::max();  

    inline double color_distance(double a, double b){
    
        return 1.0/std::abs((a-b)+0.01);
    
    
    }  
    
  inline double secondTerm(double a,double b,double c, double d, double e, Ndarray<double,2> mat) {
      
      return 0.5*(a*( a*mat[0][0] + b*mat[1][0] + c*mat[2][0] + d*mat[3][0] + e*mat[4][0] ) +
                  b*( a*mat[0][1] + b*mat[1][1] + c*mat[2][1] + d*mat[3][1] + e*mat[4][1] ) +
                  c*( a*mat[0][2] + b*mat[1][2] + c*mat[2][2] + d*mat[3][2] + e*mat[4][2] ) +
                  d*( a*mat[0][3] + b*mat[1][3] + c*mat[2][3] + d*mat[3][3] + e*mat[4][3] ) +
                  e*( a*mat[0][4] + b*mat[1][4] + c*mat[2][4] + d*mat[3][4] + e*mat[4][4] ) );
  } 
    
    // compute the aux nodeID between pixels, you should always guarantee that p1_id is smaller than p2_id;
    
    inline int computeAuxNodeID( int p1_id, int p2_id, int length){
    
        //int small = std::min(p1_id,p2_id);
        //int big = std::max(p1_id,p2_id);
        
        
        
        return (2*length+1 - p1_id)*p1_id/2 + p2_id-p1_id + length+2; 
        
        // we set here the offset equals to 2, because we should take S,T into count. 
        
        
    
    }   

  class Edge{
    
  public:
    Edge(){};
    Edge(int s,int d):_source(s),_destination(d){ _capacity = 10; _flow = 0;}
    Edge(int s,int d,float c, float f):_source(s),_destination(d),_capacity(c),_flow(f){}

      ~Edge(){/*std::cout<< "Edge Destructor" << std::endl;*/};

    inline int    from()           const { return _source;} 
    inline int    to()             const { return _destination;}
    inline float  capacity()       const { return _capacity;}
    inline void  setFlow(int f)         { _flow = f;}
    inline float  flow()           const { return _flow;}
    inline int    other(int &node) const { return ( node == _source ? _destination : _source);}
    
    bool operator==(const Edge &e) const{ return (this->_source == e._source && this->_destination == e._destination);}  
    bool operator <(const Edge &e) const{ return ( (this->_source * 2500 + this->_destination) < (e._source * 2500+ e._destination));}  
      
      
    float residualCapacityTo(const int node) const{
      if (node==_source) return _flow;
      if (node == _destination) return _capacity-_flow;
      
        return -1;
    }
      
    void addResidualFlowTo(const int node, float delta){
      
      if ( node == _source) _flow -= delta;
      else if( node == _destination) _flow += delta;
    
    }
    bool isSaturated(){ return _capacity==_flow;}
      
      
  private:
    int  _source;
    int  _destination;
    float _capacity;
    float _flow;

    friend std::ostream& operator<<(std::ostream& os, const Edge &e); 
   
  };
  
   std::ostream& operator<< (std::ostream& os, const Edge &e) 
   {
     os <<"from " << e._source  << "  to " <<   e._destination<< " capacity "<< e._capacity << " flow " <<  e._flow << std::endl;
     return os;
   } 
      
  class Graph{
    
  public:
    Graph(){};
    ~Graph(){};
   
    int N_edges() const { return _edges.size()/2;}  
      
    int N_nodes() const {
      std::multimap<int, boost::shared_ptr<Edge> >::size_type n = 0 ;
      std::multimap<int, boost::shared_ptr<Edge> >::const_iterator iter;
      for(iter = _edges.begin() ; iter != _edges.end() ; iter = _edges.upper_bound(iter->first) ) ++n ;
      return n;
    }
      
    inline void add_edge(int s, int d, int c, int f){    
    
      boost::shared_ptr<Edge> e(new Edge(s,d,c,0));  
      _edges.insert(std::pair< int, boost::shared_ptr<Edge> >(s, e));
      _edges.insert(std::pair< int, boost::shared_ptr<Edge> >(d, e));
        
    }
    inline void add_edge(const boost::shared_ptr<Edge> &e){
        
        _edges.insert(std::pair< int, boost::shared_ptr<Edge> >(e->from(), e));
        _edges.insert(std::pair< int, boost::shared_ptr<Edge> >(e->to(),   e));
        
      }
      
    void del_edge(const int &s, const int &d){
      
        std::multimap<int,boost::shared_ptr<Edge> >::iterator iter;
        std::multimap<int,boost::shared_ptr<Edge> >::iterator iter_end;
        
        iter =  _edges.find(s);
        iter_end =  _edges.upper_bound(s);
        
        for (; iter != iter_end ; iter++) {
            if ( (iter->second)->to() == d) _edges.erase(iter);
            break;
        }
        
        iter =  _edges.find(d);
        iter_end =  _edges.upper_bound(d);
        
        for (; iter != iter_end ; iter++) {
            if ( (iter->second)->from() == d) _edges.erase(iter);
            break;
        }
    }  
    void adjacent(int Node, std::list < boost::shared_ptr<Edge> > &adjs){
      
        adjs.clear();
 
      std::multimap<int,boost::shared_ptr<Edge> >::const_iterator iter = _edges.find(Node);
      int count = _edges.count(Node);
      while(count!=0){
	
          adjs.push_back(iter->second);
          iter++;
          count--;
      }
      
    }
      
      std::multimap<int,boost::shared_ptr<Edge> > get_edges(){ return _edges;}
  private:   
    

      
    std::multimap<int,boost::shared_ptr<Edge> > _edges;
    


  };
    
    class graphBuilder{
    
    protected:
        
        Graph _G;
    
    public:
        
        Graph& graph(){
        
            return _G;
        }
        
        void add_edge(const std::list< boost::shared_ptr<Edge> > &e_list){
            
            
            BOOST_FOREACH( boost::shared_ptr<Edge>  e, e_list )
            {
                _G.add_edge(e);
            }
            
        }

    };

    
    

 
   class MRFGraphBuilder: public graphBuilder{
    
   public:
       
       void buildEdgesFromImage( 
                                Ndarray<double,2>  &R_ND,
                                Ndarray<double,2>  &G_ND,
                                Ndarray<double,2>  &B_ND,
                                Ndarray<double,2>  &L_ND,
                                Ndarray<double,2>  &labelArray_ND,
                                Ndarray<double,2>  &miuArray_ND,
                                Ndarray<double,3>  &inversedCovarMatrArray_ND,
                                Ndarray<double,1>  &lnCovarMatrixDet_ND){
           
           int n_row = R_ND.getShape(0);
           int n_line  = R_ND.getShape(1);
           float V;
           int S   = n_row * n_line  ;
           int T = n_row * n_line+1;  
           float pott = BETA;
        
           
           
           /////////////////////////////////
           //building Edges between pixels//
           /////////////////////////////////
           
           //   * <-> * <-> * <-> *
           //   ^     ^     ^     ^
           //   |     |     |     |
           //   v     v     v     v
           //   * <-> * <-> * <-> * 
           //   ^     ^     ^     ^
           //   |     |     |     |
           //   v     v     v     v
           //   * <-> * <-> * <-> * 
           //   ^     ^     ^     ^
           //   |     |     |     |
           //   v     v     v     v
           //   * <-> * <-> * <-> * 
              
           // each edge has the same weight beta*1 
           
           std::list< boost::shared_ptr<Edge> > e_list;
           std::cout << R_ND[0][0] << " "
           <<G_ND[0][0]<< " "
           <<B_ND[0][0]<< " "<<std::endl;
           
           //from 0 to n_row-1 and from 0 to n_line-1
           #ifdef DEBUG
           std::cout << miuArray_ND[0][0] << " "
                    <<miuArray_ND[0][1]<< " "
                    <<miuArray_ND[0][2]<< " "
                    <<miuArray_ND[0][3]<< " "
                    <<miuArray_ND[0][4]<<std::endl;

           std::cout << miuArray_ND[1][0] << " "
           <<miuArray_ND[1][1]<< " "
           <<miuArray_ND[1][2]<< " "
           <<miuArray_ND[1][3]<< " "
           <<miuArray_ND[1][4]<<std::endl;
            #endif
           for(int i =1; i < (n_row-1); i++ ){
           
               for(int j =1; j < (n_line-1); j++ ){
                
                   boost::shared_ptr<Edge> e1(new Edge(i+j*n_row,i+1+j*n_row  ,color_distance(L_ND[i][j],L_ND[i+1][j]),0));
                   boost::shared_ptr<Edge> e2(new Edge(i+j*n_row,i-1+j*n_row  ,color_distance(L_ND[i][j],L_ND[i-1][j]),0));
                   
                   boost::shared_ptr<Edge> e3(new Edge(i+j*n_row,i+(j+1)*n_row,color_distance(L_ND[i][j],L_ND[i][j+1]),0));
                   boost::shared_ptr<Edge> e4(new Edge(i+j*n_row,i+(j-1)*n_row,color_distance(L_ND[i][j],L_ND[i][j-1]),0));
                   

                   
                   V = ALPHA * ( lnCovarMatrixDet_ND[0] + secondTerm( float(i)   - miuArray_ND[0][0], 
                                                                      float(j)   - miuArray_ND[0][1], 
                                                                      R_ND[i][j] - miuArray_ND[0][2],
                                                                      G_ND[i][j] - miuArray_ND[0][3],
                                                                      B_ND[i][j] - miuArray_ND[0][4],
                                                                      inversedCovarMatrArray_ND[0])) + offset ;
                   
     

                   boost::shared_ptr<Edge> e5(new Edge(S,i+j*n_row,V,0)); 

                   V = ALPHA *( lnCovarMatrixDet_ND[1] + secondTerm( float(i)   - miuArray_ND[1][0], 
                                                                     float(j)   - miuArray_ND[1][1], 
                                                                     R_ND[i][j] - miuArray_ND[1][2],
                                                                     G_ND[i][j] - miuArray_ND[1][3],
                                                                     B_ND[i][j] - miuArray_ND[1][4],
                                                                     inversedCovarMatrArray_ND[1])) + offset;
       

                   boost::shared_ptr<Edge> e6(new Edge(i+j*n_row,T,V,0)); 
                   
                   graphBuilder::_G.add_edge(e1);
                   graphBuilder::_G.add_edge(e2);
                   graphBuilder::_G.add_edge(e3);
                   graphBuilder::_G.add_edge(e4);
                   graphBuilder::_G.add_edge(e5);  
                   graphBuilder::_G.add_edge(e6);
          
               }
           }
       
           for(int i =1; i < (n_row-1); i++ ){
               
               int j = (n_line-1);
               
               boost::shared_ptr<Edge> e1(new Edge(i+j*n_row,i+1+j*n_row  ,color_distance(L_ND[i][j],L_ND[i+1][j]),0));
               boost::shared_ptr<Edge> e2(new Edge(i+j*n_row,i-1+j*n_row  ,color_distance(L_ND[i][j],L_ND[i-1][j]),0));
               boost::shared_ptr<Edge> e3(new Edge(i+j*n_row,i+(j - 1)*n_row,color_distance(L_ND[i][j],L_ND[i][j-1]),0));

               
               V = ALPHA * (lnCovarMatrixDet_ND[0] + secondTerm(float(i)   - miuArray_ND[0][0], 
                                                                float(j)   - miuArray_ND[0][1], 
                                                                R_ND[i][j] - miuArray_ND[0][2],
                                                                G_ND[i][j] - miuArray_ND[0][3],
                                                                B_ND[i][j] - miuArray_ND[0][4],
                                                                inversedCovarMatrArray_ND[0])) + offset;
               
               boost::shared_ptr<Edge> e4(new Edge(S,i+j*n_row,V,0)); 
  
               
               V = ALPHA * (lnCovarMatrixDet_ND[1] + secondTerm(float(i)   - miuArray_ND[1][0], 
                                                                float(j)   - miuArray_ND[1][1], 
                                                                R_ND[i][j] - miuArray_ND[1][2],
                                                                G_ND[i][j] - miuArray_ND[1][3],
                                                                B_ND[i][j] - miuArray_ND[1][4],
                                                                inversedCovarMatrArray_ND[1])) + offset;
               
               boost::shared_ptr<Edge> e5(new Edge(i+j*n_row,T,V,0)); 
               
               graphBuilder::_G.add_edge(e1);
               graphBuilder::_G.add_edge(e2);
               graphBuilder::_G.add_edge(e3);
               graphBuilder::_G.add_edge(e4);
               graphBuilder::_G.add_edge(e5);
               
           }
           for(int i =1; i < (n_row-1); i++ ){
               
               int j = 0;
               
               boost::shared_ptr<Edge> e1(new Edge(i,i+1  ,color_distance(L_ND[i][0],L_ND[i+1][0]),0));
               boost::shared_ptr<Edge> e2(new Edge(i,i-1  ,color_distance(L_ND[i][0],L_ND[i-1][0]),0));
               boost::shared_ptr<Edge> e3(new Edge(i,i + n_row  ,color_distance(L_ND[i][j],L_ND[0][1]),0));
               

               
               V = ALPHA * (lnCovarMatrixDet_ND[0] + secondTerm(float(i)  - miuArray_ND[0][0], 
                                                               float(j)  - miuArray_ND[0][1], 
                                                               R_ND[i][j] - miuArray_ND[0][2],
                                                               G_ND[i][j] - miuArray_ND[0][3],
                                                               B_ND[i][j] - miuArray_ND[0][4],
                                                               inversedCovarMatrArray_ND[0])) + offset;
               
               boost::shared_ptr<Edge> e4(new Edge(S,i+j*n_row,V,0)); 
                
               
               V = ALPHA * (lnCovarMatrixDet_ND[1] + secondTerm(float(i)  - miuArray_ND[1][0], 
                                                               float(j)  - miuArray_ND[1][1], 
                                                               R_ND[i][j] - miuArray_ND[1][2],
                                                               G_ND[i][j] - miuArray_ND[1][3],
                                                               B_ND[i][j] - miuArray_ND[1][4],
                                                               inversedCovarMatrArray_ND[1]))+ offset;
               
               boost::shared_ptr<Edge> e5(new Edge(i+j*n_row,T,V,0)); 
               
               graphBuilder::_G.add_edge(e1);
               graphBuilder::_G.add_edge(e2);
               graphBuilder::_G.add_edge(e3);
               graphBuilder::_G.add_edge(e4); 
               graphBuilder::_G.add_edge(e5);
               
           }
           for(int j =1; j < (n_line-1); j++ ){
               
               int i = n_row - 1;
               
               boost::shared_ptr<Edge> e1(new Edge(n_row-1+j*n_row,n_row-1+(j+1)*n_row,color_distance(L_ND[i][j],L_ND[i][j+1]),0));
               boost::shared_ptr<Edge> e2(new Edge(n_row-1+j*n_row,n_row-1+(j-1)*n_row,color_distance(L_ND[i][j],L_ND[i][j-1]),0));
               boost::shared_ptr<Edge> e3(new Edge(n_row-1+j*n_row,n_row-2+j*n_row,color_distance(L_ND[i][j],L_ND[i-1][j]),0));
               
               
               V = ALPHA * ( lnCovarMatrixDet_ND[0] + secondTerm(float(i)  - miuArray_ND[0][0], 
                                                               float(j)  - miuArray_ND[0][1], 
                                                               R_ND[i][j] - miuArray_ND[0][2],
                                                               G_ND[i][j] - miuArray_ND[0][3],
                                                               B_ND[i][j] - miuArray_ND[0][4],
                                                               inversedCovarMatrArray_ND[0]) )+offset ;
               
               boost::shared_ptr<Edge> e4(new Edge(S,i+j*n_row,V,0)); 
               
               
               V = ALPHA * (lnCovarMatrixDet_ND[1] + secondTerm(float(i)  - miuArray_ND[1][0], 
                                                               float(j)  - miuArray_ND[1][1], 
                                                               R_ND[i][j] - miuArray_ND[1][2],
                                                               G_ND[i][j] - miuArray_ND[1][3],
                                                               B_ND[i][j] - miuArray_ND[1][4],
                                                               inversedCovarMatrArray_ND[1]) )+offset;
               boost::shared_ptr<Edge> e5(new Edge(i+j*n_row,T,V,0)); 
               
               graphBuilder::_G.add_edge(e1);
               graphBuilder::_G.add_edge(e2);
               graphBuilder::_G.add_edge(e3);
               graphBuilder::_G.add_edge(e4);  
               graphBuilder::_G.add_edge(e5);
               
           }
           
           for(int j =1; j < (n_line-1); j++ ){
               
               int i = 0;
               
               boost::shared_ptr<Edge> e1(new Edge(j*n_row,(j+1)*(n_row),color_distance(L_ND[0][j],L_ND[0][j+1]),0));
               boost::shared_ptr<Edge> e2(new Edge(j*n_row,(j-1)*(n_row),color_distance(L_ND[0][j],L_ND[0][j-1]),0));
               boost::shared_ptr<Edge> e3(new Edge(j*n_row,j*(n_row)+1,color_distance(L_ND[0][j],L_ND[1][j]),0));
              
               
               V = ALPHA * (lnCovarMatrixDet_ND[0] + secondTerm(float(i)  - miuArray_ND[0][0], 
                                                               float(j)  - miuArray_ND[0][1], 
                                                               R_ND[i][j] - miuArray_ND[0][2],
                                                               G_ND[i][j] - miuArray_ND[0][3],
                                                               B_ND[i][j] - miuArray_ND[0][4],
                                                               inversedCovarMatrArray_ND[0]))+offset;
               boost::shared_ptr<Edge> e4(new Edge(S,i+j*n_row,V,0)); 
                
               
               V = ALPHA * (lnCovarMatrixDet_ND[1] + secondTerm(float(i)  - miuArray_ND[1][0], 
                                                               float(j)  - miuArray_ND[1][1], 
                                                               R_ND[i][j] - miuArray_ND[1][2],
                                                               G_ND[i][j] - miuArray_ND[1][3],
                                                               B_ND[i][j] - miuArray_ND[1][4],
                                                               inversedCovarMatrArray_ND[1]))+offset;
               boost::shared_ptr<Edge> e5(new Edge(i+j*n_row,T,V,0)); 
               graphBuilder::_G.add_edge(e1);
               graphBuilder::_G.add_edge(e2);
               graphBuilder::_G.add_edge(e3);
               graphBuilder::_G.add_edge(e4); 
               graphBuilder::_G.add_edge(e5);
               
           }
           
           //0,0 upper-left
           boost::shared_ptr<Edge> e1(new Edge(0,1,color_distance(L_ND[0][0],L_ND[0][1]),0));
           boost::shared_ptr<Edge> e2(new Edge(0,n_row,color_distance(L_ND[0][0],L_ND[0][1]),0));
           
           //n_row-1,0 upper-right
           boost::shared_ptr<Edge> e3(new Edge(n_row-1,n_row-2,color_distance(L_ND[n_row-1][0],L_ND[n_row-2][0]),0));
           boost::shared_ptr<Edge> e4(new Edge(n_row-1,n_row-1+n_row,color_distance(L_ND[n_row-1][0],L_ND[n_row-1][1]),0));
           
           //0, n_line-1 bottom left
           boost::shared_ptr<Edge> e5(new Edge((n_line-1)*n_row,(n_line-1)*n_row+1,color_distance(L_ND[0][n_line-1],L_ND[1][n_line-1]),0));
           boost::shared_ptr<Edge> e6(new Edge((n_line-1)*n_row,(n_line-2)*n_row,color_distance(L_ND[0][n_line-1],L_ND[0][n_line-2]),0));
           
           
           //n_row-1, n_line-1 bottom right
           boost::shared_ptr<Edge> e7(new Edge(n_row-1 + (n_line-1)*n_row,n_row-2 + (n_line-1)*n_row,color_distance(L_ND[n_row-1][n_line-1],L_ND[n_row-2][n_line-1]),0));
           boost::shared_ptr<Edge> e8(new Edge(n_row-1 + (n_line-1)*n_row,n_row-1 + (n_line-2)*n_row,color_distance(L_ND[n_row-1][n_line-1],L_ND[n_row-1][n_line-2]),0));
           
           // 0,0
           int i =0;
           int j =0;
           
           V = ALPHA * (lnCovarMatrixDet_ND[0] + secondTerm(float(i)  - miuArray_ND[0][0], 
                                                           float(j)  - miuArray_ND[0][1], 
                                                           R_ND[i][j] - miuArray_ND[0][2],
                                                           G_ND[i][j] - miuArray_ND[0][3],
                                                           B_ND[i][j] - miuArray_ND[0][4],
                                                           inversedCovarMatrArray_ND[0]))+offset;
           boost::shared_ptr<Edge> e9(new Edge(S,i+j*n_row,V,0)); 
            
           
           V = ALPHA * (lnCovarMatrixDet_ND[1] + secondTerm(float(i)  - miuArray_ND[1][0], 
                                                           float(j)  - miuArray_ND[1][1], 
                                                           R_ND[i][j] - miuArray_ND[1][2],
                                                           G_ND[i][j] - miuArray_ND[1][3],
                                                           B_ND[i][j] - miuArray_ND[1][4],
                                                           inversedCovarMatrArray_ND[1]))+offset;
           boost::shared_ptr<Edge> e10(new Edge(i+j*n_row,T,V,0)); 
           
           
           // n_row-pott,0
           i = n_row-1;
           j = 0;
           
           V = ALPHA * (lnCovarMatrixDet_ND[0] + secondTerm(float(i)  - miuArray_ND[0][0], 
                                                           float(j)  - miuArray_ND[0][1], 
                                                           R_ND[i][j] - miuArray_ND[0][2],
                                                           G_ND[i][j] - miuArray_ND[0][3],
                                                           B_ND[i][j] - miuArray_ND[0][4],
                                                           inversedCovarMatrArray_ND[0]))+offset;
           boost::shared_ptr<Edge> e11(new Edge(S,i+j*n_row,V,0)); 
           V = ALPHA * (lnCovarMatrixDet_ND[1] + secondTerm(float(i)  - miuArray_ND[1][0], 
                                                           float(j)  - miuArray_ND[1][1], 
                                                           R_ND[i][j] - miuArray_ND[1][2],
                                                           G_ND[i][j] - miuArray_ND[1][3],
                                                           B_ND[i][j] - miuArray_ND[1][4],
                                                           inversedCovarMatrArray_ND[1]))+offset;
           boost::shared_ptr<Edge> e12(new Edge(i+j*n_row,T,V,0)); 
           
           // 0,n_line - 1
           i = 0;
           j = n_line - 1;
           
           V = ALPHA *( lnCovarMatrixDet_ND[0] + secondTerm(float(i)  - miuArray_ND[0][0], 
                                                           float(j)  - miuArray_ND[0][1], 
                                                           R_ND[i][j] - miuArray_ND[0][2],
                                                           G_ND[i][j] - miuArray_ND[0][3],
                                                           B_ND[i][j] - miuArray_ND[0][4],
                                                           inversedCovarMatrArray_ND[0]))+offset;
           boost::shared_ptr<Edge> e13(new Edge(S,i+j*n_row,V,0)); 
           
           
           V = ALPHA * (lnCovarMatrixDet_ND[1] + secondTerm(float(i)  - miuArray_ND[1][0], 
                                                           float(j)  - miuArray_ND[1][1], 
                                                           R_ND[i][j] - miuArray_ND[1][2],
                                                           G_ND[i][j] - miuArray_ND[1][3],
                                                           B_ND[i][j] - miuArray_ND[1][4],
                                                           inversedCovarMatrArray_ND[1]))+offset;
           boost::shared_ptr<Edge> e14(new Edge(i+j*n_row,T,V,0)); 
           
           
           // n_row-1,n_line - 1
           i = n_row-1;
           j = n_line - 1;
           
           V = ALPHA * (lnCovarMatrixDet_ND[0] + secondTerm(float(i) - miuArray_ND[0][0], 
                                                           float(j) - miuArray_ND[0][1], 
                                                           R_ND[i][j] - miuArray_ND[0][2],
                                                           G_ND[i][j] - miuArray_ND[0][3],
                                                           B_ND[i][j] - miuArray_ND[0][4],
                                                           inversedCovarMatrArray_ND[0]))+offset;
           boost::shared_ptr<Edge> e15(new Edge(S,i+j*n_row,V,0)); 
           
           V = ALPHA *( lnCovarMatrixDet_ND[1] + secondTerm(float(i) - miuArray_ND[1][0], 
                                                           float(j) - miuArray_ND[1][1], 
                                                           R_ND[i][j] - miuArray_ND[1][2],
                                                           G_ND[i][j] - miuArray_ND[1][3],
                                                           B_ND[i][j] - miuArray_ND[1][4],
                                                           inversedCovarMatrArray_ND[1]))+offset;
           
           boost::shared_ptr<Edge> e16(new Edge(i+j*n_row,T,V,0)); 
           
           graphBuilder::_G.add_edge(e1);
           graphBuilder::_G.add_edge(e2);
           graphBuilder::_G.add_edge(e3);
           graphBuilder::_G.add_edge(e4);
           graphBuilder::_G.add_edge(e5);
           graphBuilder::_G.add_edge(e6);
           graphBuilder::_G.add_edge(e7);
           graphBuilder::_G.add_edge(e8);
           graphBuilder::_G.add_edge(e9); 

           graphBuilder::_G.add_edge(e11);  
           graphBuilder::_G.add_edge(e10);
           graphBuilder::_G.add_edge(e12);
           graphBuilder::_G.add_edge(e13);  
           graphBuilder::_G.add_edge(e14);
           graphBuilder::_G.add_edge(e15);  
           graphBuilder::_G.add_edge(e16);
           
           
       }
        
       void buildEdgesForExpansionMove(int label,
                                       Ndarray<double,2>  &R_ND,
                                       Ndarray<double,2>  &G_ND,
                                       Ndarray<double,2>  &B_ND,
                                       Ndarray<double,2>  &labelArray_ND,
                                       Ndarray<double,2>  &miuArray_ND,
                                       Ndarray<double,3>  &covarMatrArray_ND,
                                       Ndarray<double,1>  &lnCovarMatrixDet_ND){
       
           
           int n_row = R_ND.getShape(0);
           int n_line  = R_ND.getShape(1);
           
           
           int currentPixelLabel;
           
           
           int nodeAlpha  = n_row * n_line  ; // S
           int node_NonAlpha = n_row * n_line+1; // T
           
           int auxNodeCount = n_row * n_line+2;
           int aux_node = 0;
           
           
           float aux_capacity; // Dp(alpha) = ln( sqrt( 8*pi**3 ) * det(covar_matrix) )
           
           double x_miu = miuArray_ND[label][0];
           double y_miu = miuArray_ND[label][1];
           double r_miu = miuArray_ND[label][2];
           double g_miu = miuArray_ND[label][3];
           double b_miu = miuArray_ND[label][4];
           
           
           std::map< int,int > auxNode_map;
           
           Ndarray<double,2> covarMat = covarMatrArray_ND[label];
           
           for(int i =1; i < (n_row-1); i++ ){
               
               for(int j =1; j < (n_line-1); j++ ){
                   
                   // t_alpha_p
                   
                   aux_capacity = lnCovarMatrixDet_ND[label] + secondTerm( i-x_miu , 
                                                                           j-y_miu, 
                                                                           R_ND[i][j] - r_miu, 
                                                                           G_ND[i][j] - g_miu, 
                                                                           B_ND[i][j] - b_miu, 
                                                                           covarMat);
                   
                   boost::shared_ptr<Edge> t_alpha_p(new Edge(nodeAlpha, i + j * n_row, aux_capacity, 0));
                   graphBuilder::_G.add_edge(t_alpha_p);
                   
                   //t_non_Alpah_p
                   currentPixelLabel = labelArray_ND[i][j];
                   
                   aux_capacity = ( currentPixelLabel == label ?  
                                    POSITIVE_INFINITY :
                                    lnCovarMatrixDet_ND[currentPixelLabel] + secondTerm(i- miuArray_ND[currentPixelLabel][0], 
                                                                                        j- miuArray_ND[currentPixelLabel][1], 
                                                                                        R_ND[i][j] - miuArray_ND[currentPixelLabel][2], 
                                                                                        G_ND[i][j] - miuArray_ND[currentPixelLabel][3], 
                                                                                        B_ND[i][j] - miuArray_ND[currentPixelLabel][4], 
                                                                                        covarMat));
                   
                   boost::shared_ptr<Edge> t_c_alpha_p(new Edge(i + j * n_row, node_NonAlpha, aux_capacity, 0));
                   
                   graphBuilder::_G.add_edge(t_c_alpha_p);
                   

                   
                   if (labelArray_ND[i][j] == labelArray_ND[i][j+1]) {// no aux node
                       
                       boost::shared_ptr<Edge> e_pq(new Edge(i+j*n_row,i+(j+1)*n_row, POTTS(currentPixelLabel,label), 0));
                       graphBuilder::_G.add_edge(e_pq);
                       
                   }else{ // insert aux node
                       
                       //aux_node = computeAuxNodeID(i+j*n_row,i+(j+1)*n_row,length);
                       
                       
                       
                       int tmp = j*(2*n_row - 1)+ n_row - 1 +  i; // this line calculate a unique ID from(i,j) and (i,j+1), this is the ID of the aux_node between them.
                       if (auxNode_map.find(tmp) == auxNode_map.end()) auxNode_map[tmp] = auxNodeCount++;
                       
                       aux_node = auxNode_map[tmp];
                       
                       if (aux_node == 3284) std::cout << "NO!!!!" << std::endl;
                       std::cout << i << "  "  <<  " "  << j << " vers bas  " << aux_node   << std::endl; 
                       
                       
                       boost::shared_ptr<Edge> e_pa(new Edge(i+j*n_row,aux_node, POTTS(labelArray_ND[i][j],label),  0));
                       boost::shared_ptr<Edge> e_aq(new Edge(aux_node,i+(j+1)*n_row, POTTS(labelArray_ND[i][j+1],label),0));
                       
                       boost::shared_ptr<Edge> t_c_alpha_a(new Edge(aux_node,node_NonAlpha, 1,0));
                       
                       graphBuilder::_G.add_edge(e_pa);
                       graphBuilder::_G.add_edge(e_aq);
                       graphBuilder::_G.add_edge(t_c_alpha_a);
                   
                   
                   }
                   
                   if (labelArray_ND[i][j] == labelArray_ND[i+1][j]) { // no aux node
                       boost::shared_ptr<Edge> e_pq(new Edge(i+j*n_row,i+1+j*n_row, POTTS(currentPixelLabel,label), 0));
                       graphBuilder::_G.add_edge(e_pq);
                       
                   }else{ // insert aux node
                       
                       //aux_node = computeAuxNodeID(i+j*n_row,i+1+j*n_row,length);
                       //aux_node = i+j*n_row + offset;
                       
                       int tmp = j*(2*n_row - 1) + i; // this line calculate a unique ID from(i,j) and (i,j+1), this is the ID of the aux_node between them.
                       if (auxNode_map.find(tmp) == auxNode_map.end()) auxNode_map[tmp] = auxNodeCount++;
                       
                       aux_node = auxNode_map[tmp];
                       

                       
                       boost::shared_ptr<Edge> e_pa(new Edge(i+j*n_row,aux_node, POTTS(labelArray_ND[i][j],label),  0));
                       boost::shared_ptr<Edge> e_aq(new Edge(aux_node,i+1+j*n_row, POTTS(labelArray_ND[i+1][j],label),0));
                       
                       boost::shared_ptr<Edge> t_c_alpha_a(new Edge(aux_node,node_NonAlpha, 1,0));
                       
                       graphBuilder::_G.add_edge(e_pa);
                       graphBuilder::_G.add_edge(e_aq);
                       graphBuilder::_G.add_edge(t_c_alpha_a);
                   
                   
                   }
                   
                   if (labelArray_ND[i][j] == labelArray_ND[i][j-1]) { // no aux node
                       
                       boost::shared_ptr<Edge> e_pq(new Edge(i+j*n_row,i+(j-1)*n_row, POTTS(currentPixelLabel,label), 0));
                       graphBuilder::_G.add_edge(e_pq);
                       
                   }else{ // insert aux node
                   
                       //aux_node = computeAuxNodeID(i+(j-1)*n_row,i+j*n_row,length);
                       //aux_node = i+j*n_row + offset;
                       
                       
                       int tmp = (j-1)*(2*n_row - 1) + n_row - 1+ i; // this line calculate a unique ID from(i,j) and (i,j+1), this is the ID of the aux_node between them.
                       if (auxNode_map.find(tmp) == auxNode_map.end()) auxNode_map[tmp] = auxNodeCount++;
                       
                       aux_node = auxNode_map[tmp];
                       

                       
                       boost::shared_ptr<Edge> e_pa(new Edge(i+j*n_row,aux_node, POTTS(labelArray_ND[i][j],label),  0));
                       boost::shared_ptr<Edge> e_aq(new Edge(aux_node,i+(j-1)*n_row, POTTS(labelArray_ND[i][j-1],label),0));
                       boost::shared_ptr<Edge> t_c_alpha_a(new Edge(aux_node,node_NonAlpha, 1,0));
                       
                       graphBuilder::_G.add_edge(e_pa);
                       graphBuilder::_G.add_edge(e_aq);
                       graphBuilder::_G.add_edge(t_c_alpha_a);
                   
                   }
                   
                   if (labelArray_ND[i][j] == labelArray_ND[i-1][j]) { // no aux node
                       
                       boost::shared_ptr<Edge> e_pq(new Edge(i+j*n_row,i-1+j*n_row, POTTS(currentPixelLabel,label), 0));
                       graphBuilder::_G.add_edge(e_pq);
                       
                   }else{ // insert aux node
                       
                       //aux_node = computeAuxNodeID(i-1+j*n_row,i+j*n_row,length);
                       //aux_node = i-1+j*n_row + offset;
                   
                       int tmp = j*(2*n_row - 1) + i -1  ; // this line calculate a unique ID from(i,j) and (i,j+1), this is the ID of the aux_node between them.
                       if (auxNode_map.find(tmp) == auxNode_map.end()) auxNode_map[tmp] = auxNodeCount++;
                       
                       aux_node = auxNode_map[tmp];
                       

                       
                       boost::shared_ptr<Edge> e_pa(new Edge(i+j*n_row,aux_node, POTTS(labelArray_ND[i][j],label),  0));
                       boost::shared_ptr<Edge> e_aq(new Edge(aux_node,i-1+j*n_row, POTTS(labelArray_ND[i-1][j],label),0));
                       boost::shared_ptr<Edge> t_c_alpha_a(new Edge(aux_node,node_NonAlpha, 1,0));
                       
                       graphBuilder::_G.add_edge(e_pa);
                       graphBuilder::_G.add_edge(e_aq);
                       graphBuilder::_G.add_edge(t_c_alpha_a);
                   
                   
                   }
                   
                   
               }
           }
           {
               int i =0;
               int j =0;
           
               aux_capacity = lnCovarMatrixDet_ND[label] + secondTerm(-x_miu , 
                                                                  -y_miu, 
                                                                  R_ND[i][j] - r_miu, 
                                                                  G_ND[i][j] - g_miu, 
                                                                  B_ND[i][j] - b_miu, 
                                                                  covarMat);
           
               boost::shared_ptr<Edge> t_alpha_p(new Edge(nodeAlpha, i + j * n_row, aux_capacity, 0));
               graphBuilder::_G.add_edge(t_alpha_p);
               
               currentPixelLabel = labelArray_ND[i][j];
           
               aux_capacity = ( currentPixelLabel == label ?  
                               POSITIVE_INFINITY :
                               lnCovarMatrixDet_ND[currentPixelLabel] + secondTerm(i- miuArray_ND[currentPixelLabel][0], 
                                                                               j- miuArray_ND[currentPixelLabel][1], 
                                                                               R_ND[i][j] - miuArray_ND[currentPixelLabel][2], 
                                                                               G_ND[i][j] - miuArray_ND[currentPixelLabel][3], 
                                                                               B_ND[i][j] - miuArray_ND[currentPixelLabel][4], 
                                                                               covarMat));
           
               boost::shared_ptr<Edge> t_c_alpha_p(new Edge(0, node_NonAlpha, aux_capacity, 0));
           
               graphBuilder::_G.add_edge(t_c_alpha_p);
               
               
               boost::shared_ptr<Edge> e_pq1(new Edge(0,1, POTTS(currentPixelLabel,label), 0));
               graphBuilder::_G.add_edge(e_pq1);
               
               boost::shared_ptr<Edge> e_pq2(new Edge(0,n_row, POTTS(currentPixelLabel,label), 0));
               graphBuilder::_G.add_edge(e_pq2);
           }
           {
               int i =n_row-1;
               int j =0;
               
               aux_capacity = lnCovarMatrixDet_ND[label] + secondTerm(i-x_miu , 
                                                                      j-y_miu, 
                                                                      R_ND[i][j] - r_miu, 
                                                                      G_ND[i][j] - g_miu, 
                                                                      B_ND[i][j] - b_miu, 
                                                                      covarMat);
               
               boost::shared_ptr<Edge> t_alpha_p(new Edge(nodeAlpha, i + j * n_row, aux_capacity, 0));
               graphBuilder::_G.add_edge(t_alpha_p);
               
               currentPixelLabel = labelArray_ND[i][j];
               
               aux_capacity = ( currentPixelLabel == label ?  
                               POSITIVE_INFINITY :
                               lnCovarMatrixDet_ND[currentPixelLabel] + secondTerm(i- miuArray_ND[currentPixelLabel][0], 
                                                                                   j- miuArray_ND[currentPixelLabel][1], 
                                                                                   R_ND[i][j] - miuArray_ND[currentPixelLabel][2], 
                                                                                   G_ND[i][j] - miuArray_ND[currentPixelLabel][3], 
                                                                                   B_ND[i][j] - miuArray_ND[currentPixelLabel][4], 
                                                                                   covarMat));
               
               boost::shared_ptr<Edge> t_c_alpha_p(new Edge(n_row-1, node_NonAlpha, aux_capacity, 0));
               
               graphBuilder::_G.add_edge(t_c_alpha_p);
               
               
               boost::shared_ptr<Edge> e_pq1(new Edge(n_row-1,n_row-2, POTTS(currentPixelLabel,label), 0));
               graphBuilder::_G.add_edge(e_pq1);
               
               boost::shared_ptr<Edge> e_pq2(new Edge(n_row-1,2*n_row-1, POTTS(currentPixelLabel,label), 0));
               graphBuilder::_G.add_edge(e_pq2);
           } 
           {
               int i =0;
               int j =n_line-1;
               
               aux_capacity = lnCovarMatrixDet_ND[label] + secondTerm(i-x_miu , 
                                                                      j-y_miu, 
                                                                      R_ND[i][j] - r_miu, 
                                                                      G_ND[i][j] - g_miu, 
                                                                      B_ND[i][j] - b_miu, 
                                                                      covarMat);
               
               boost::shared_ptr<Edge> t_alpha_p(new Edge(nodeAlpha, i + j * n_row, aux_capacity, 0));
               graphBuilder::_G.add_edge(t_alpha_p);
               
               currentPixelLabel = labelArray_ND[i][j];
               
               aux_capacity = ( currentPixelLabel == label ?  
                               POSITIVE_INFINITY :
                               lnCovarMatrixDet_ND[currentPixelLabel] + secondTerm(i- miuArray_ND[currentPixelLabel][0], 
                                                                                   j- miuArray_ND[currentPixelLabel][1], 
                                                                                   R_ND[i][j] - miuArray_ND[currentPixelLabel][2], 
                                                                                   G_ND[i][j] - miuArray_ND[currentPixelLabel][3], 
                                                                                   B_ND[i][j] - miuArray_ND[currentPixelLabel][4], 
                                                                                   covarMat));
               
               boost::shared_ptr<Edge> t_c_alpha_p(new Edge(i+j*n_row, node_NonAlpha, aux_capacity, 0));
               
               graphBuilder::_G.add_edge(t_c_alpha_p);
               
               
               boost::shared_ptr<Edge> e_pq1(new Edge(j*n_row,(j-1)*n_row, POTTS(currentPixelLabel,label), 0));
               graphBuilder::_G.add_edge(e_pq1);
               
               boost::shared_ptr<Edge> e_pq2(new Edge(j*n_row,1+j*n_row, POTTS(currentPixelLabel,label), 0));
               graphBuilder::_G.add_edge(e_pq2);
           }
           {
               int i =n_row-1;
               int j =n_line-1;
               
               aux_capacity = lnCovarMatrixDet_ND[label] + secondTerm(i-x_miu , 
                                                                      j-y_miu, 
                                                                      R_ND[i][j] - r_miu, 
                                                                      G_ND[i][j] - g_miu, 
                                                                      B_ND[i][j] - b_miu, 
                                                                      covarMat);
               
               boost::shared_ptr<Edge> t_alpha_p(new Edge(nodeAlpha, i + j * n_row, aux_capacity, 0));
               graphBuilder::_G.add_edge(t_alpha_p);
               
               currentPixelLabel = labelArray_ND[i][j];
               
               aux_capacity = ( currentPixelLabel == label ?  
                               POSITIVE_INFINITY :
                               lnCovarMatrixDet_ND[currentPixelLabel] + secondTerm(i- miuArray_ND[currentPixelLabel][0], 
                                                                                   j- miuArray_ND[currentPixelLabel][1], 
                                                                                   R_ND[i][j] - miuArray_ND[currentPixelLabel][2], 
                                                                                   G_ND[i][j] - miuArray_ND[currentPixelLabel][3], 
                                                                                   B_ND[i][j] - miuArray_ND[currentPixelLabel][4], 
                                                                                   covarMat));
               
               boost::shared_ptr<Edge> t_c_alpha_p(new Edge(i+j*n_row, node_NonAlpha, aux_capacity, 0));
               
               graphBuilder::_G.add_edge(t_c_alpha_p);
               
               
               boost::shared_ptr<Edge> e_pq1(new Edge(i+j*n_row,i+j*n_row-1, POTTS(currentPixelLabel,label), 0));
               graphBuilder::_G.add_edge(e_pq1);
               
               boost::shared_ptr<Edge> e_pq2(new Edge(i+j*n_row,i+(j-1)*n_row, POTTS(currentPixelLabel,label), 0));
               graphBuilder::_G.add_edge(e_pq2);
           }
       }
   };
  class FordFulkerson {

  public:
    
    FordFulkerson(Graph &G, int s, int t){
        
        _maxflow = 0;
        _counter = 0;
        
        double bottle = POSITIVE_INFINITY;
        
        while(hasAugmentingPathFromS2T(G,s,t)){
            
            
            bottle = POSITIVE_INFINITY;
            
            for (int v = t; v !=s ;  v=edgeTo[v]->other(v)) {
                    
                float resCap = edgeTo[v]->residualCapacityTo(v);
                bottle = (bottle < resCap ? bottle:resCap);
            }
            
            
            for (int v = t; v !=s ;  v=edgeTo[v]->other(v)) {
                
                edgeTo[v]->addResidualFlowTo(v,bottle);
                //std::cout << *edgeTo[v];
            }
            _maxflow += bottle;
        }

    }

    bool hasAugmentingPathFromS2T(Graph &G,int s,int t){
      
      //std::cout<< "\naugmenting path" << _counter++ << std::endl;
      marked.clear();
      marked.resize(G.N_nodes());
      
      edgeTo.clear();
      edgeTo.resize(G.N_nodes());
      
      marked[s] = true;

      std::deque<int> q;
      
      q.push_back(s);

      while(!q.empty() && !marked[t]){

          int v = q.front();
          q.pop_front();
          std::list< boost::shared_ptr<Edge> > adjs;
          G.adjacent(v,adjs);
          std::list< boost::shared_ptr<Edge> >::iterator e_iter;
          

          for(e_iter = adjs.begin();
              e_iter!=adjs.end();
              e_iter++
              )
          {
              
              int w = (*e_iter)->other(v);
              
              if((*e_iter)->residualCapacityTo(w) > 0 && !marked[w]){
   
                  edgeTo[w] = *e_iter;
                  marked[w] = true;
                  if (w==t) break;
                  q.push_back(w);

              }
              
          }
        
      }

        return marked[t];
    };
    
      float getMaxflow(){return _maxflow;};
  private:
    std::vector<bool> marked;
    std::vector< boost::shared_ptr<Edge> > edgeTo;
    float _maxflow;
      int _counter;
  };
  
    class SearchTree{
        
    private:
        
        std::vector< int > _parents;
        std::vector< int > _tree;
        
        std::set< int > _activeNodes;
        
        std::vector<boost::shared_ptr<Edge> > _edgeFromSTo;
        
        std::vector<boost::shared_ptr<Edge> > _edgeFromTTo;
        
        std::set< int > _orphans;
        
        std::list< boost::shared_ptr<Edge> > _adjsOfCurrentActiveNode;
        
        
        int _S;
        int _T;
        int _curentActiveNode;
        
        float max_flow;
        
    public:
        SearchTree(Graph &G,int S, int T, int N_nodes ){
            
            this->_S = S;
            this->_T = T;
            #ifdef DEBUG
            std::cout << "G has N_nodes :" << N_nodes << " "<< G.N_nodes() << std::endl;
            #endif
            _parents.resize(N_nodes);
            
            for(size_t i = 0; i < _parents.size();i++) _parents[i] = -1;
            
            _tree.resize(N_nodes);
            
            for(size_t i = 0; i < _tree.size();i++) _tree[i] = -1;
            
            _edgeFromSTo.resize(N_nodes);
            _edgeFromTTo.resize(N_nodes);
            
            _activeNodes.insert(_S);
            _activeNodes.insert(_T);
            
            _tree[_S] = _S;
            _tree[_T] = _T;
            
            max_flow = 0;

            _curentActiveNode = -1;
            
            while (true) {
                
                int g = growth(G); 
                
                if(  g == -1 ) break;
                
                augment(g);
                adopt(G);
                #ifdef DEGUB
                char s;
                std::cin >> s;
                #endif
            }
        }
        
        inline float getMaxflow(){ return max_flow;}
        
        int growth(Graph &G){
            
            int p;
            int q;

            //std::list< boost::shared_ptr<Edge> >::iterator e_iter;
            float cap_tree;
            int p_tree;
            #ifdef DEBUG
            std::cout <<  " In Growth" << std::endl;
            #endif
            while( !_activeNodes.empty() ){

                p = *_activeNodes.begin();
                
                if( _curentActiveNode != p){
                    
                    G.adjacent(p,_adjsOfCurrentActiveNode );
                    _curentActiveNode = p;
                    
                }
                
                p_tree = _tree[p];
                
                #ifdef DEBUG
                std::cout <<"growth: activated p is " << p << " its tree is "<< p_tree << std::endl;
                #endif
                
                //for (e_iter;e_iter != _adjsOfCurrentActiveNode.end();e_iter++){
                std::list< boost::shared_ptr<Edge> >::iterator iter=_adjsOfCurrentActiveNode.begin(); 
                
                while( ! _adjsOfCurrentActiveNode.empty() ){

                    boost::shared_ptr < Edge > ptr = _adjsOfCurrentActiveNode.front();
                    _adjsOfCurrentActiveNode.pop_front();
                    //iter++;
                    q = ptr -> other(p);
                  
                    cap_tree = ( p_tree == _S  ? (ptr)->residualCapacityTo(q) : (ptr)->residualCapacityTo(p));
                    
                    #ifdef DEBUG
                    std::cout << "growth: "<<*(ptr);
                    std::cout << "growth: cap_tree  "<< p<< " to " << q << " is " << cap_tree << std::endl; 
                    #endif
                    
                    if( cap_tree > 0){
                        
                        #ifdef DEBUG   
                        std::cout << "growth: tree of " << q << " is "<<  _tree[q] << std::endl;   
                        #endif
                        
                        if ( _tree[q] == -1 ) {
                                
                            _tree[q] = _tree[p];
                            
                            #ifdef DEBUG   
                            std::cout << "growth: parent of " << q << " is "<<  p << std::endl;   
                            #endif
                            
                            _parents[q]= p;
                            _activeNodes.insert(q);
                                
                            if ( p_tree == _S) _edgeFromSTo[q] = ptr; 
                            else if ( p_tree == _T) _edgeFromTTo[q] = ptr; 
                            
                            #ifdef DEBUG 
                            std::cout << "growth: cap_tree > 0 "<<*(ptr);
                            #endif
                            
                        }else if( _tree[p] != _tree[q]){
                                
                            if ( p_tree == _S ) _edgeFromSTo[q] = ptr; 
                            else if ( p_tree == _T ) _edgeFromTTo[q] = ptr; 
                            
                            #ifdef DEBUG
                            std::cout << "growth: return q  "<< q << std::endl; 
                            #endif
                            
                            return q;
                        }
                        
                    }
                    
                }
                #ifdef DEBUG
                std::cout << "growth: erase p "<< p << std::endl;
                #endif
                _activeNodes.erase(_activeNodes.find(p));
            }
            return -1;
        }
        
        void augment(int q){
            
            float bottle = POSITIVE_INFINITY;

            for (int v = q; v !=_S ;  v=_edgeFromSTo[v]->other(v)) {
                
                float resCap = _edgeFromSTo[v]->residualCapacityTo(v);
                bottle = (bottle < resCap ? bottle:resCap);
                #ifdef DEBUG
                std::cout << "augment: finding bottle neck from S "<<*_edgeFromSTo[v];
                #endif

                
            }     
    
            for (int v = q; v != _T;  v=_edgeFromTTo[v]->other(v)) {  
                
                float resCap = _edgeFromTTo[v]->residualCapacityTo(_edgeFromTTo[v]->other(v));
                bottle = (bottle < resCap ? bottle:resCap);
                #ifdef DEBUG
                std::cout << "augment: finding bottle neck from T "<<*_edgeFromTTo[v];
                #endif
            }
            
            max_flow += bottle;
            #ifdef DEBUG
            std::cout << "augment: bottle neck is "<<bottle<<std::endl;
            #endif
            for (int v = q; v !=_S ;  v=_edgeFromSTo[v]->other(v)) {
                
                _edgeFromSTo[v]->addResidualFlowTo(v,bottle);
                
                //if ( _edgeFromSTo[v]->isSaturated() ) {
                if ( _edgeFromSTo[v]->residualCapacityTo(v) == 0 ) {  
                    #ifdef DEBUG
                    std::cout << "augment: edge is saturated from S " <<*_edgeFromSTo[v];
                    #endif
                    _parents[v] = -1;  
                    _orphans.insert(v);
    
                }
            }
            
            for (int v = q; v !=_T ;  v=_edgeFromTTo[v]->other(v)) {
                
                _edgeFromTTo[v]->addResidualFlowTo(_edgeFromTTo[v]->other(v),bottle);

                //if ( _edgeFromTTo[v]->isSaturated() ) { 
                if ( _edgeFromTTo[v]->residualCapacityTo(_edgeFromTTo[v]->other(v)) == 0 ) { 
                    #ifdef DEBUG
                    std::cout << "augment: edge is saturated from T " <<*_edgeFromTTo[v];
                    #endif
                    _parents[v] = -1;    
                    _orphans.insert(v);
                }
            }
        }
        
        void adopt(Graph &G){
            
            
            float cap_tree;
            int q;
            bool newParentFound = false;
            int p ;
            while(!_orphans.empty()){
            
                newParentFound = false;
                p = *_orphans.begin();

                _orphans.erase(_orphans.find(p));
                
                if( p==_S || p==_T ){
                    continue;
                
                }
                #ifdef DEBUG
                std::cout << "adopt: orphan p is "<< p << std::endl;
                #endif


                std::list< boost::shared_ptr<Edge> > adjs;
                G.adjacent(p,adjs);
                std::list< boost::shared_ptr<Edge> >::iterator iter; 
                
                for(iter= adjs.begin(); iter != adjs.end(); iter++){

                    q = (*iter)->other(p);
                    #ifdef DEBUG
                    std::cout << "adopt: p's adj "<< q << std::endl;
                    #endif
                    cap_tree = ( _tree[q] == _S ? (*iter)->residualCapacityTo(p) : (*iter)->residualCapacityTo(q));
                   
                    if( _tree[q] == _tree[p] && cap_tree > 0  ){
                
                        if ( originOf(q) == _tree[p]) {
                            
                            _parents[p] = q;
                            if ( _tree[p] == _S) _edgeFromSTo[p] = *iter; 
                            else if ( _tree[p] == _T) _edgeFromTTo[p] = *iter; 
                            #ifdef DEBUG
                            std::cout << "adopt: new parent of "<< p << " is " << q<< std::endl;
                            #endif
                            newParentFound = true;
                            break;
                        }
                    }
                                     
                }
                if (!newParentFound){
                    #ifdef DEBUG
                    std::cout << "adopt: no valid parent of "<< p << " is found" << std::endl;
                    #endif  
                    for(iter= adjs.begin(); iter != adjs.end(); iter++){
                        
                        q = (*iter)->other(p); 
                        #ifdef DEBUG
                        std::cout << "adopt: "<< q << " its tree is " << _tree[q] << std::endl;
                        #endif  
                        if( _tree[q] == _tree[p]){
                        
                            cap_tree = ( _tree[p] == _S ? (*iter)->residualCapacityTo(p) : (*iter)->residualCapacityTo(q));
                            
                            if( cap_tree > 0  ){
                                
                                _activeNodes.insert(q);
                                
                            }
                            
                            if (_parents[q]==p){
                                #ifdef DEBUG
                                std::cout << "adopt: p's adj "<< q << " is added to orphans" << std::endl;
                                #endif
                                _orphans.insert(q);
                                _parents[q] = -1;
                            }



                        }
                    } 
                
                    
                    if( p!=_S && p!=_T ){
                        _tree[p] = -1;
                                
                        #ifdef DEBUG
                        std::cout << "adopt: "<< p <<" now is free" << std::endl;
                        #endif
                    }
                    
                    if (_activeNodes.find(p) != _activeNodes.end()) {
                        
                        #ifdef DEBUG
                        std::cout << "adopt: erase "<< p <<" from activeNodes" << std::endl;
                        #endif
                        
                        _activeNodes.erase(_activeNodes.find(p));
                            
                    }
                    
                }
                
            }
            
        
        }
        int originOf(int p){
            
            int q = _parents[p];
            int pre = -1;
            while(q != -1){
                
                pre = q;
                q = _parents[q];
            }
            #ifdef DEBUG
            std::cout <<"origin of "<< p << " is "<< pre << std::endl;
            #endif
            return pre;
        }
        
    };
    
    void reachableFromS(Graph &G, std::vector< bool > &reachibility, int S){
        
        int currentNode;
        
        for(size_t i =0; i < reachibility.size();i++){
        
            reachibility[i] = false;
            
        }
        
        std::list< int > nodesToVisit;
        
        nodesToVisit.push_back(S);
        
        std::list< boost::shared_ptr<Edge> >::iterator e_iter;
        
        reachibility[S] = true;
        
        while (!nodesToVisit.empty()) {
            
            currentNode = nodesToVisit.front();
            nodesToVisit.pop_front();
            
            std::list< boost::shared_ptr<Edge> > adjs;
            G.adjacent(currentNode,adjs);
            
            e_iter = adjs.begin();
            
            for(;e_iter != adjs.end();e_iter++){
            
                if (!(*e_iter)->isSaturated()){
                
                    if ((*e_iter)->from() == currentNode) {
                        
                        int reached = (*e_iter)->to();
                        
                        if (reachibility[reached]==false){
                            reachibility[reached] = true;
                            nodesToVisit.push_back(reached);
                        }
                    }
                }
            }
        }
        
        
    
    
    
    }
    
    float alphaExpansionQuickGraphCut(numpyArray<double> channel_R,
                                     numpyArray<double> channel_G,
                                     numpyArray<double> channel_B,
                                     numpyArray<double> labelArray,
                                     numpyArray<double> miuArray, 
                                     numpyArray<double> inversedCovarMatArray, 
                                     numpyArray<double> lnCovarMatDet,
                                     numpyArray<double> activeContour){
        std::cout << POSITIVE_INFINITY << std::endl;
        
        Ndarray<double,2> R_ND(channel_R);   
        Ndarray<double,2> G_ND(channel_G);   
        Ndarray<double,2> B_ND(channel_B);
        
        std::cout<< R_ND[0][0]<< " " <<G_ND[0][0]<< " "<<B_ND[0][0]<< std::endl;
        
        Ndarray<double,2> labelArray_ND(labelArray);
        Ndarray<double,2> miuArray_ND(miuArray);
        Ndarray<double,3> inversedCovarMatrArray_ND(inversedCovarMatArray);
        Ndarray<double,1> lnCovarMatrixDet_ND(lnCovarMatDet);
        Ndarray<double,2> activeContour_ND(activeContour);
       
        MRFGraphBuilder builder;

        std::cout << "building graph" << std::endl;
        builder.buildEdgesForExpansionMove( 0,
                                            R_ND,
                                            G_ND,
                                            B_ND,
                                            labelArray_ND,
                                            miuArray_ND,
                                            inversedCovarMatrArray_ND,
                                            lnCovarMatrixDet_ND);
        
        std::cout << "building graph ends" << std::endl;
        
        Graph &g = builder.graph();
        
        int n_row = R_ND.getShape(0);
        int n_line  = R_ND.getShape(1);
        
        int k = n_row * n_line;
        
        std::cout << "searching" << std::endl;
        
        SearchTree search_tree(g,k,k+1,g.N_nodes());
        
        std::cout << "searching ends" << std::endl;
        
        std::cout << search_tree.getMaxflow() <<std::endl;
        
        std::list< boost::shared_ptr<Edge> > adjs ;
        
        g.adjacent(k,adjs);
        
        std::list< boost::shared_ptr<Edge> >::iterator e_iter = adjs.begin();
        
        for ( ; e_iter != adjs.end();e_iter++){
            int y = (*e_iter)->to()/n_row;
            int x = (*e_iter)->to() - n_row*y;
            labelArray_ND[x][y] = ((*e_iter)->isSaturated()? 0:1) ;
            
        }
        

        

        return search_tree.getMaxflow();  
        
    
    }
    

    
    int quickGraphCut(  int n_seg,
                    numpyArray<double> channel_R,
                      numpyArray<double> channel_G,
                      numpyArray<double> channel_B,
                      numpyArray<double> channel_L,
                    numpyArray<double> labelArray,
                    numpyArray<double> miuArray, 
                    numpyArray<double> inversedCovarMatArray, 
                    numpyArray<double> lnCovarMatDet)
    {
        Ndarray<double,2> R_ND(channel_R);   
        Ndarray<double,2> G_ND(channel_G);   
        Ndarray<double,2> B_ND(channel_B);   
        Ndarray<double,2> L_ND(channel_B);
      
      std::cout<< R_ND[0][0]<< " " <<G_ND[0][0]<< " "<<B_ND[0][0]<< std::endl;

      Ndarray<double,2> labelArray_ND(labelArray);
      Ndarray<double,2> miuArray_ND(miuArray);
      Ndarray<double,3> inversedCovarMatrArray_ND(inversedCovarMatArray);
      Ndarray<double,1> lnCovarMatrixDet_ND(lnCovarMatDet);

      MRFGraphBuilder builder;

      std::cout << " building begins" <<std::endl;
      
      builder.buildEdgesFromImage(R_ND,
                                  G_ND,
                                  B_ND,
                                  L_ND,
                                  labelArray_ND,
                                  miuArray_ND,
                                  inversedCovarMatrArray_ND,
                                  lnCovarMatrixDet_ND);
      
      std::cout << " building ends" <<std::endl;
        
      Graph &baseGraph = builder.graph();

      
      int n_row = R_ND.getShape(0);
      int n_line  = R_ND.getShape(1);
      
      int k = n_row * n_line;

      std::cout << " search begins"<< std::endl;
      if (1){
      
      
          SearchTree search_tree(baseGraph,k,k+1,k+2);
          
          std::cout << search_tree.getMaxflow() <<std::endl;
          
          std::list< boost::shared_ptr<Edge> > adjs ;
          
          baseGraph.adjacent(k,adjs);
          
          std::list< boost::shared_ptr<Edge> >::iterator e_iter = adjs.begin();
          
          std::vector<bool> reachable;
          
          reachable.resize(n_row * n_line+2);
          
          
          reachableFromS(baseGraph,reachable, k);
          
          for (int j = 0; j < k;j++){
          
              int y = j / n_row;
              int x = j - n_row*y;
              labelArray_ND[x][y] = (reachable[j] ? 1:0) ;
              
          
          }
          /*
          for ( ; e_iter != adjs.end();e_iter++){
              int y = (*e_iter)->to()/n_row;
              int x = (*e_iter)->to() - n_row*y;
              labelArray_ND[x][y] = ((*e_iter)->isSaturated()? 0:1) ;
                            
          }
        
          std::cout << " search ends"<< std::endl;
          
           baseGraph.adjacent(k+1,adjs);
          
          e_iter = adjs.begin();
          
          
          for (; e_iter != adjs.end();e_iter++){
              //if (marked[(*e_iter)->from()]==0){
                  int y = std::floor((*e_iter)->from()/n_row);
                  int x = (*e_iter)->from() - n_row*y;
                  //labelArray_ND[x][y] = ((*e_iter)->isSaturated()? 1:0) ;
              //}
              
          }
          */
          return search_tree.getMaxflow();  
          
      
      }else{
      
          
          FordFulkerson FF(baseGraph,
                           R_ND.getShape(0)*R_ND.getShape(1),
                           R_ND.getShape(0)*R_ND.getShape(1)+1);
          

          
          std::cout << FF.getMaxflow() <<std::endl;
          
          
          int n_row = R_ND.getShape(0);
          int n_line  = R_ND.getShape(1);
          
          int k = n_row * n_line;
          
          std::list< boost::shared_ptr<Edge> > adjs;
          baseGraph.adjacent(k,adjs);
          
          std::list< boost::shared_ptr<Edge> >::iterator e_iter = adjs.begin();
          
          
          for (; e_iter != adjs.end();e_iter++){
              int y = (*e_iter)->to()/n_row;
              int x = (*e_iter)->to() - n_row*y;
              //std::cout << x << "  "  << y << std::endl;
              labelArray_ND[x][y] = ((*e_iter)->isSaturated()? 1:0) ;
              
          }
          
          baseGraph.adjacent(k+1,adjs);
          
          e_iter = adjs.begin();
          
          
          for (; e_iter != adjs.end();e_iter++){
              int y = std::floor((*e_iter)->from()/n_row);
              int x = (*e_iter)->from() - n_row*y;
              //std::cout << x << "  "  << y << std::endl;
              labelArray_ND[x][y] = ((*e_iter)->isSaturated()? 1:0) ;
              
          }
          
          
          std::cout << " search ends"<< std::endl;
          
          return FF.getMaxflow();
      
      }
      
      

  }
  
    
    // Estimation/Maximation Process
    double Determinant(Ndarray<double,2> a,int n)
    {
        int i,j,j1,j2;
        double det = 0;
        
        
        if (n < 1) { /* Error */
            
        } else if (n == 1) { /* Shouldn't get used */
            det = a[0][0];
        } else if (n == 2) {
            det = a[0][0] * a[1][1] - a[1][0] * a[0][1];
        } else {
            det = 0;
            for (j1=0;j1<n;j1++) {
                Ndarray<double,2> m(a);                
                //m = std::malloc((n-1)*sizeof(double *));
                //for (i=0;i<n-1;i++)
                //    m[i] = std::malloc((n-1)*sizeof(double));
                for (i=1;i<n;i++) {
                    j2 = 0;
                    for (j=0;j<n;j++) {
                        if (j == j1)
                            continue;
                        m[i-1][j2] = a[i][j];
                        j2++;
                    }
                }
                det += pow(-1.0,1.0+j1+1.0) * a[0][j1] * Determinant(m,n-1);
                //for (i=0;i<n-1;i++)
                //    free(m[i]);
                //free(m);
            }
        }
        return(det);
    }
    
    inline float proba_dl_thetaM(float a,float b,float c,float d, float e,Ndarray<double,2> inversedCovarMatrix, float covarMatrixDet){
           
        return std::exp( - secondTerm(a,b,c,d,e,inversedCovarMatrix)) / ( std::sqrt(std::pow(2*3.1415926,5) * covarMatrixDet) );
    
    }
    
    float  prob_m_dl(int m, Ndarray<double,1> pi_m, float x, float y, float r, float g, float b, Ndarray<double,2> &miuArray,Ndarray<double,3> &inversedCovarMatrix, Ndarray<double,1>&covarMatrixDet ){
        
        float tmp2 = 1;
        
        float tmp1 = pi_m[m] * proba_dl_thetaM(x-miuArray[m][0],y-miuArray[m][1],r-miuArray[m][2],g-miuArray[m][3],b-miuArray[m][4],inversedCovarMatrix[m],covarMatrixDet[m]); 
                     
        tmp2 = ( m == 0 ? tmp1 + pi_m[1] * proba_dl_thetaM(x-miuArray[1][0],y-miuArray[1][1],r-miuArray[1][2],g-miuArray[1][3],b-miuArray[1][4],inversedCovarMatrix[1],covarMatrixDet[1]):
                          tmp1 + pi_m[0] * proba_dl_thetaM(x-miuArray[0][0],y-miuArray[0][1],r-miuArray[0][2],g-miuArray[0][3],b-miuArray[0][4],inversedCovarMatrix[0],covarMatrixDet[0]));
        

        return float(tmp1)/float(tmp2);
    
    }
    
    void EMProcess(numpyArray<double> pi_m,
                   numpyArray<double> new_pi_m,
                   numpyArray<double> channel_R,
                   numpyArray<double> channel_G,
                   numpyArray<double> channel_B,
                   numpyArray<double> miuArray,
                   numpyArray<double> new_miuArray,
                   numpyArray<double> inversedCovarMatArray,
                   numpyArray<double> new_covarMatArray, 
                   numpyArray<double> covarMatDet ){
    
        Ndarray<double,1> pi_m_ND(pi_m);   
        Ndarray<double,1> new_pi_m_ND(new_pi_m); 
        
        Ndarray<double,2> R_ND(channel_R);   
        Ndarray<double,2> G_ND(channel_G); 
        Ndarray<double,2> B_ND(channel_B);    
        
        Ndarray<double,2> miuArray_ND(miuArray);
        Ndarray<double,2> new_miuArray_ND(new_miuArray);
        
        Ndarray<double,3> inversedCovarMatArray_ND(inversedCovarMatArray);    
        Ndarray<double,3> new_covarMatArray_ND(new_covarMatArray);
        
        Ndarray<double,1> covarMatrixDet_ND(covarMatDet);
        
        std::cout<< miuArray_ND[0][0] << std::endl; 
        
        std::cout<< miuArray_ND[0][1] << std::endl; 
        
        std::cout<< miuArray_ND[1][0] << std::endl; 
        
        std::cout<< miuArray_ND[1][1] << std::endl; 
        
        float post_prob=0,sum=0;
        

        //there are only 2 lables so far

        for (int m = 0; m < 2; m++){
            
            std::cout<<m<<"   " <<miuArray_ND[m][1] << std::endl; 
            
            // init of new_covarMatArray and new_miuArray
            for(int p = 0; p < new_covarMatArray_ND[m].getShape(0);p++){
                for(int q = 0; q < new_covarMatArray_ND[m].getShape(1);q++){
                    
                    new_covarMatArray_ND[m][p][q] =0;
                    
                }        
                
            }
            for(int p = 0; p < new_miuArray_ND[m].getShape(0);p++){
            
                new_miuArray_ND[m][p] = 0;
            }
            
            
            std::cout<<m<<"   " <<miuArray_ND[m][1] << std::endl; 
            
            for (int i  = 0; i < R_ND.getShape(0);i++){
                for (int j = 0; j < R_ND.getShape(1);j++){
                    
                    post_prob = prob_m_dl(m,pi_m_ND,i,j,R_ND[i][j],G_ND[i][j],B_ND[i][j], miuArray_ND,inversedCovarMatArray_ND,covarMatrixDet_ND);
                    
                    //if ( i == 0 && j == 0) std::cout<< R_ND[i][j]<< " " <<G_ND[i][j]<< " "<<B_ND[i][j] << std::endl; 

                    sum += post_prob;
                    
                    new_miuArray_ND[m][0] += i * post_prob;
                    new_miuArray_ND[m][1] += j * post_prob;
                    new_miuArray_ND[m][2] += R_ND[i][j] * post_prob;
                    new_miuArray_ND[m][3] += G_ND[i][j] * post_prob;
                    new_miuArray_ND[m][4] += B_ND[i][j] * post_prob;
                    
                    std::vector < double > dl;
                    dl.resize(5);
                    
                    dl[0] = i          - miuArray_ND[m][0];
                    dl[1] = j          - miuArray_ND[m][1];
                    dl[2] = R_ND[i][j] - miuArray_ND[m][2];
                    dl[3] = G_ND[i][j] - miuArray_ND[m][3];
                    dl[4] = B_ND[i][j] - miuArray_ND[m][4];
                    
                    for(int p = 0; p < new_covarMatArray_ND[m].getShape(0);p++){
                        for(int q = 0; q < new_covarMatArray_ND[m].getShape(1);q++){
                            
                            new_covarMatArray_ND[m][p][q] = new_covarMatArray_ND[m][p][q] + dl[p]*dl[q]*post_prob;
                        
                        }        
                    
                    }
                    
                    
                    
                }        
            }
        
            
            new_miuArray_ND[m][0] = new_miuArray_ND[m][0] / sum;
            new_miuArray_ND[m][1] = new_miuArray_ND[m][1] / sum;
            new_miuArray_ND[m][2] = new_miuArray_ND[m][2] / sum;
            new_miuArray_ND[m][3] = new_miuArray_ND[m][3] / sum;
            new_miuArray_ND[m][4] = new_miuArray_ND[m][4] / sum;
            
            for(int p = 0; p < new_covarMatArray_ND[m].getShape(0);p++){
                for(int q = 0; q < new_covarMatArray_ND[m].getShape(1);q++){
                    
                    new_covarMatArray_ND[m][p][q] = new_covarMatArray_ND[m][p][q]  / sum;
                    
                }        
                
            }
            new_pi_m_ND[m] = double(sum) / double(R_ND.getShape(0) * R_ND.getShape(1) );           
        }
        
    
    }
  int MRF(numpyArray<double> array1, numpyArray<double> array2)
  {
    Ndarray<double,3> a(array1);
    Ndarray<double,3> b(array2);
    
    double sum=0.0;
      
      for (int i = 0; i < a.getShape(0); i++)
      {
          for (int j = 0; j < a.getShape(1); j++)
          {
              for (int k = 0; k < a.getShape(2); k++)
              {
                  std::cout << a[i][j][k]  << std::endl;
                  sum += a[i][j][k];
              }
          }
      }
      for (int i = 0; i < a.getShape(0); i++)
      {
          for (int j = 0; j < a.getShape(1); j++)
          {
              for (int k = 0; k < a.getShape(2); k++)
              {
                  std::cout << b[i][j][k] << std::endl;
              }
          }
      }
    return sum;    
  }
    
    
} 
